/*
 * GLMatrix.cpp
 *
 *  Created on: 2012-03-08
 *      Author: Aco
 */

#include "GLMatrix.h"

#include <cmath>
#include <cstdlib>
#include <cstring>

const GLMatrix GLMatrix::Identity;

void matmul4_c(const float m0[16], const float m1[16], float d[16])
{
		d[0] = m0[0]*m1[0] + m0[4]*m1[1] + m0[8]*m1[2] + m0[12]*m1[3];
		d[1] = m0[1]*m1[0] + m0[5]*m1[1] + m0[9]*m1[2] + m0[13]*m1[3];
		d[2] = m0[2]*m1[0] + m0[6]*m1[1] + m0[10]*m1[2] + m0[14]*m1[3];
		d[3] = m0[3]*m1[0] + m0[7]*m1[1] + m0[11]*m1[2] + m0[15]*m1[3];
		d[4] = m0[0]*m1[4] + m0[4]*m1[5] + m0[8]*m1[6] + m0[12]*m1[7];
		d[5] = m0[1]*m1[4] + m0[5]*m1[5] + m0[9]*m1[6] + m0[13]*m1[7];
		d[6] = m0[2]*m1[4] + m0[6]*m1[5] + m0[10]*m1[6] + m0[14]*m1[7];
		d[7] = m0[3]*m1[4] + m0[7]*m1[5] + m0[11]*m1[6] + m0[15]*m1[7];
		d[8] = m0[0]*m1[8] + m0[4]*m1[9] + m0[8]*m1[10] + m0[12]*m1[11];
		d[9] = m0[1]*m1[8] + m0[5]*m1[9] + m0[9]*m1[10] + m0[13]*m1[11];
		d[10] = m0[2]*m1[8] + m0[6]*m1[9] + m0[10]*m1[10] + m0[14]*m1[11];
		d[11] = m0[3]*m1[8] + m0[7]*m1[9] + m0[11]*m1[10] + m0[15]*m1[11];
		d[12] = m0[0]*m1[12] + m0[4]*m1[13] + m0[8]*m1[14] + m0[12]*m1[15];
		d[13] = m0[1]*m1[12] + m0[5]*m1[13] + m0[9]*m1[14] + m0[13]*m1[15];
		d[14] = m0[2]*m1[12] + m0[6]*m1[13] + m0[10]*m1[14] + m0[14]*m1[15];
		d[15] = m0[3]*m1[12] + m0[7]*m1[13] + m0[11]*m1[14] + m0[15]*m1[15];
}

void matmul4_neon(const float m0[16], const float m1[16], float d[16])
{
#ifdef __MATH_NEON
        asm volatile (
        "vld1.32                {d0, d1}, [%1]!                 \n\t"   //q0 = m1
        "vld1.32                {d2, d3}, [%1]!                 \n\t"   //q1 = m1+4
        "vld1.32                {d4, d5}, [%1]!                 \n\t"   //q2 = m1+8
        "vld1.32                {d6, d7}, [%1]                  \n\t"   //q3 = m1+12
        "vld1.32                {d16, d17}, [%0]!               \n\t"   //q8 = m0
        "vld1.32                {d18, d19}, [%0]!               \n\t"   //q9 = m0+4
        "vld1.32                {d20, d21}, [%0]!               \n\t"   //q10 = m0+8
        "vld1.32                {d22, d23}, [%0]                \n\t"   //q11 = m0+12

        "vmul.f32               q12, q8, d0[0]                  \n\t"   //q12 = q8 * d0[0]
        "vmul.f32               q13, q8, d2[0]                  \n\t"   //q13 = q8 * d2[0]
        "vmul.f32               q14, q8, d4[0]                  \n\t"   //q14 = q8 * d4[0]
        "vmul.f32               q15, q8, d6[0]                  \n\t"   //q15 = q8 * d6[0]
        "vmla.f32               q12, q9, d0[1]                  \n\t"   //q12 = q9 * d0[1]
        "vmla.f32               q13, q9, d2[1]                  \n\t"   //q13 = q9 * d2[1]
        "vmla.f32               q14, q9, d4[1]                  \n\t"   //q14 = q9 * d4[1]
        "vmla.f32               q15, q9, d6[1]                  \n\t"   //q15 = q9 * d6[1]
        "vmla.f32               q12, q10, d1[0]                 \n\t"   //q12 = q10 * d0[0]
        "vmla.f32               q13, q10, d3[0]                 \n\t"   //q13 = q10 * d2[0]
        "vmla.f32               q14, q10, d5[0]                 \n\t"   //q14 = q10 * d4[0]
        "vmla.f32               q15, q10, d7[0]                 \n\t"   //q15 = q10 * d6[0]
        "vmla.f32               q12, q11, d1[1]                 \n\t"   //q12 = q11 * d0[1]
        "vmla.f32               q13, q11, d3[1]                 \n\t"   //q13 = q11 * d2[1]
        "vmla.f32               q14, q11, d5[1]                 \n\t"   //q14 = q11 * d4[1]
        "vmla.f32               q15, q11, d7[1]                 \n\t"   //q15 = q11 * d6[1]

        "vst1.32                {d24, d25}, [%2]!               \n\t"   //d = q12
        "vst1.32                {d26, d27}, [%2]!               \n\t"   //d+4 = q13
        "vst1.32                {d28, d29}, [%2]!               \n\t"   //d+8 = q14
        "vst1.32                {d30, d31}, [%2]                \n\t"   //d+12 = q15

        : "+r"(m0), "+r"(m1), "+r"(d) :
    : "q0", "q1", "q2", "q3", "q8", "q9", "q10", "q11", "q12", "q13", "q14", "q15",
        "memory"
        );
#else
        matmul4_c(m0, m1, d);
#endif
}

void matvec4_c(const float m[16], const float v[4], float d[4])
{
        d[0] = m[0]*v[0] + m[4]*v[1] + m[8]*v[2] + m[12]*v[3];
        d[1] = m[1]*v[0] + m[5]*v[1] + m[9]*v[2] + m[13]*v[3];
        d[2] = m[2]*v[0] + m[6]*v[1] + m[10]*v[2] + m[14]*v[3];
        d[3] = m[3]*v[0] + m[7]*v[1] + m[11]*v[2] + m[15]*v[3];
}

void matvec4_neon(const float m[16], const float v[4], float d[4])
{
#ifdef __MATH_NEON
        asm volatile (
        "vld1.32                {d0, d1}, [%1]                  \n\t"   //Q0 = v
        "vld1.32                {d18, d19}, [%0]!               \n\t"   //Q1 = m
        "vld1.32                {d20, d21}, [%0]!               \n\t"   //Q2 = m+4
        "vld1.32                {d22, d23}, [%0]!               \n\t"   //Q3 = m+8
        "vld1.32                {d24, d25}, [%0]!               \n\t"   //Q4 = m+12

        "vmul.f32               q13, q9, d0[0]                  \n\t"   //Q5 = Q1*Q0[0]
        "vmla.f32               q13, q10, d0[1]                 \n\t"   //Q5 += Q1*Q0[1]
        "vmla.f32               q13, q11, d1[0]                 \n\t"   //Q5 += Q2*Q0[2]
        "vmla.f32               q13, q12, d1[1]                 \n\t"   //Q5 += Q3*Q0[3]

        "vst1.32                {d26, d27}, [%2]                \n\t"   //Q4 = m+12
        :
        : "r"(m), "r"(v), "r"(d)
    : "q0", "q9", "q10","q11", "q12", "q13", "memory"
        );
#else
        matvec4_c(m, v, d);
#endif
}

void GLMatrix::setIdentity()
{
	//for(int i = 0; i < 16; ++i) { m[i] = ((i / 4) == (i % 4)); }
	memset(m, 0, 64);
	m[0] = 1.0f;
	m[5] = 1.0f;
	m[10] = 1.0f;
	m[15] = 1.0f;
}
void GLMatrix::copyFrom(const GLMatrix & g)
{
	//for(int i = 0; i < 16; ++i) m[i] = g.m[i];
	memcpy(m, g.m, 64);
}

void GLMatrix::setToTranslation(float x, float y, float z)
{
	m[12] = x;
	m[13] = y;
	m[14] = z;
}

void GLMatrix::setToTranslation(const Vec3D & v)
{
	m[12] = v.X;
	m[13] = v.Y;
	m[14] = v.Z;
}

void GLMatrix::translate(float x, float y, float z)
{
	GLMatrix g;
	g.setToTranslation(x, y, z);
	multiplyBy(g);
}

void GLMatrix::translate(const Vec3D & v)
{
	GLMatrix g;
	g.setToTranslation(v.X, v.Y, v.Z);
	multiplyBy(g);
}

void GLMatrix::setToXRotation(float angle)
{
	m[5] = cos(angle);
	m[9] = -sin(angle);
	m[6] = -m[9];
	m[10] = m[5];
}

void GLMatrix::setToYRotation(float angle)
{
	m[0] = cos(angle);
	m[4] = -sin(angle);
	m[1] = -m[4];
	m[5] = m[0];
}

void GLMatrix::setToZRotation(float angle)
{
	m[0] = cos(angle);
	m[2] = -sin(angle);
	m[10] = m[0];
	m[8] = -m[2];
}

void GLMatrix::rotate(float x, float y, float z)
{
	GLMatrix g;

	g.setToZRotation(y);
	multiplyBy(g);

	g.setIdentity();
	g.setToYRotation(z);
	multiplyBy(g);

	g.setIdentity();
	g.setToXRotation(x);
	multiplyBy(g);
}

void GLMatrix::rotate(const Vec3D & v)
{
	GLMatrix g;

	g.setToZRotation(v.Y);
	multiplyBy(g);

	g.setIdentity();
	g.setToYRotation(v.Z);
	multiplyBy(g);

	g.setIdentity();
	g.setToXRotation(v.X);
	multiplyBy(g);
}

void GLMatrix::setToScale(float x, float y, float z)
{
	m[0] = x;
	m[5] = y;
	m[10] = z;
}

void GLMatrix::setToScale(const Vec3D & v)
{
	m[0] = v.X;
	m[5] = v.Y;
	m[10] = v.Z;
}

void GLMatrix::setLookAt(Vec3D eye, Vec3D lookAt, Vec3D up, Vec3D *retU, Vec3D *retR, Vec3D *retF)
{
	Vec3D f = lookAt - eye;
	f.normalizeIP();

	Vec3D s, u;
	s = f.cross(up);
	u = s.cross(f);

	Vec3D projectedTarget;
	Vec3D m_target, m_right, m_up;

	Vec3D target = lookAt - eye;
	projectedTarget = target;

	projectedTarget -= up;
	projectedTarget.normalizeIP();

	m_up = up;
	m_right = Vec3D::Zero - projectedTarget.cross(m_up);

	m_target = target;
	m_up = m_target.cross(m_right);

	m_target.normalizeIP();
	m_right.normalizeIP();
	m_up.normalizeIP();

	if(retF) *retF = m_target;
	if(retR) *retR = m_right;
	if(retU) *retU = m_up;

	m[0] = m_right.X; m[1] = m_up.X; m[2] = -m_target.X; m[3] = 0;
	m[4] = m_right.Y; m[5] = m_up.Y; m[6] = -m_target.Y; m[7] = 0;
	m[8] = m_right.Z; m[9] = m_up.Z; m[10] = -m_target.Z; m[11] = 0;
	m[12] = 0; m[13] = 0; m[14] = 0; m[15] = 1;

	GLMatrix trans;
	trans.setToTranslation(eye * -1.0f);
	multiplyBy(trans);
}

void GLMatrix::setPerspective(float left, float right, float bottom, float top, float zNear, float zFar)
{
	float A = (right + left) / (right - left);
	float B = (top + bottom) / (top - bottom);
	float C = -(zFar + zNear) / (zFar - zNear);
	float D = -2 * (zFar * zNear) / (zFar - zNear);

	m[0] = 2 * zNear / (right - left);
	m[5] = 2 * zNear / (top - bottom);
	m[8] = A;
	m[9] = B;
	m[10] = C;
	m[11] = -1.0f;
	m[14] = D;
	m[15] = 0.0f;
}

void GLMatrix::scale(float x, float y, float z)
{
	GLMatrix g;
	g.setToScale(x, y, z);
	multiplyBy(g);
}

void GLMatrix::scale(float factor)
{
	GLMatrix g;
	g.setToScale(factor, factor, factor);
	multiplyBy(g);
}

void GLMatrix::scale(const Vec3D & v)
{
	GLMatrix g;
	g.setToScale(v.X, v.Y, v.Z);
	multiplyBy(g);
}

void GLMatrix::multiplyBy(const GLMatrix & g)
{
	GLMatrix r(*this);
	matmul4_neon(m, g.m, r.m);
	copyFrom(r);
}

GLMatrix GLMatrix::getTransposed() const
{
	GLMatrix mat;
	for(int y = 0; y < 4; ++y)
		for(int x = 0; x < 4; ++x)
			mat.m[y*4 + x] = m[x*4 + y];
	return mat;
}

Vec3D GLMatrix::getAbsoluteVector() const
{
	//float v[4] = {0.0f, 0.0f, 0.0f, 1.0f};
	//float r[4];
	//matvec4_neon(m, v, r);
	//return Vec3D(r[0], r[1], r[2]);
	Vec3D v;
	v.transform(m);
	return v;
}

GLMatrix::GLMatrix() { setIdentity(); }
GLMatrix::GLMatrix(const GLMatrix & g) { copyFrom(g); }
